package behavior;


import sensor.MainColorSensorB;
import sensor.SensorColor;
import sensor.USensor;
import lejos.nxt.Button;
import lejos.nxt.ColorSensor;
import lejos.nxt.LCD;
import lejos.nxt.MotorPort;
import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.CompassHTSensor;
import lejos.nxt.comm.RConsole;

import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
import lejos.util.Delay;
//import lejos.util.TextMenu;
import motor.barredora;
import motor.carrito;

/**
 * @version   1.0

 * @author    Bruno Argenta, Esteban Arrua, Luis Galusso
 * @copyright (C) 2012 Bruno Argenta, Esteban Arrua, Luis Galusso
 * @contact   b.argenta@gmail.com, estarr2212@gmail.com, lgalusso@gmail.com
 *
 * @license        GNU/GPLv3
 * This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
   You should have received a copy of the GNU General Public License
   along with this program.  If not, see http://www.gnu.org/licenses/.
 *
 *
*/

public class BumperCar {
   public static void main(String [] args) {
	  
	  // Espero conexión BlueTooth de la PC 
	  //RConsole.openBluetooth(0);
	  
	  
	  // Creo instancia para manejar remotamente los motores y sensores del BRICK "GRP062"
	  /*RemoteNXT nxt = null;
	  NXTCommConnector connector = RS485.getConnector();
	  try {
		  nxt = new RemoteNXT("GRP062", connector);
		} catch (IOException e) {
			// TODO Auto-generated catch block			
			System.out.println("NO PUDE CONECTARME AL BRICK GRP062");
			e.printStackTrace();
		}	  
		RemoteMotor[] motors = {nxt.A, nxt.B, nxt.C};		
		RemoteSensorPort[] sensors = {nxt.S1, nxt.S2, nxt.S3, nxt.S4};
	  //NXTRegulatedMotor barredora = new NXTRegulatedMotor(MotorPort.B);
	  RemoteMotor barredora = motors[0];*/
	  // UltrasonicSensor upsensor = new UltrasonicSensor(nxt.S1);
	   
	 /* String[] menuStrings = new String[]{"Completo", "Sin colores", "Lab1"};
	  TextMenu connectionMenu = new TextMenu(menuStrings, 1);	  
	  int select = connectionMenu.select();
	  LCD.clear();
	  */
	  
	  //CREAMOS SENSOR COLOR
	 // ColorSensor		color	= new ColorSensor(SensorPort.S1);
	  SensorColor scolor = new SensorColor();
	  //CREAMOS SENSOR ULTRASONIDO
	  UltrasonicSensor 	u = new UltrasonicSensor(SensorPort.S2);
	  USensor usensor = new USensor(u);
	  //usensor.capture();
	  
	  //CREAMOS COMPAS
	  CompassHTSensor 	compas 	= new CompassHTSensor(SensorPort.S3);
	  //CREAMOS PALA
	  NXTRegulatedMotor pala = new NXTRegulatedMotor(MotorPort.B);
	  //CREAMOS CARRO
      carrito carro = new carrito();  
      
      barredora bar = new barredora();
      
      
      //usensor.continuous();
      
	  
      // La para es manejada remotamente desde GRP061
	  // RemoteMotor pala 	= motors[0];//Puerto A 
	  // RemoteSensorPort upsensor = sensors[0];
	  // NXTConnection connector = RS485.getConnector().connect("GRP062", NXTConnection.RAW);
	  
    //CALIBRAMOS EL COMPAS    
      Button.waitForAnyPress();
      compas.resetCartesianZero();      
	  compas.startCalibration();
	  carro.SetMovingSpeed(65);
	  carro.Spin();
	  Delay.msDelay(40000);
	  compas.stopCalibration();
	  carro.Stop();
	  carro.SetMovingSpeed(250);
	  
	  
	  //CALIBRAMOS SENSOR DE COLOR	  
      //scolor.calibrar();
	  
	  //POSICIONO LA PALA
      pala.resetTachoCount();
	  pala.setSpeed(100);
	  pala.rotate(-70,false);
	  LCD.drawString("PRESIONE UN BOTÓN", 0, 1);
	  Button.waitForAnyPress();
	  carro.Forward();
	  Delay.msDelay(1000);
	  carro.Stop();
	  LCD.clear();
	  pala.rotate(70,false);
	  LCD.drawString("PRESIONE UN BOTÓN", 0, 1);
	 // .println("valor inicio"+Integer.toString(usensor.getDistance()));
	  //Button.waitForAnyPress();
	  //DifferentialPilot df = new DifferentialPilot(wheelDiameter, trackWidth, leftMotor, rightMotor)
      
      //CREAMOS LA INSTANCIA DEL SENSOR DE COLOR
     //MainColorSensorB color = new MainColorSensorB(scolor);
     
      
      	  
	  // COMPORTAMIENTOS
	  Behavior tirarAzul 	= new ThrowBlueBall(scolor, carro, pala, bar);
	  Behavior tirarNaranja = new ThrowOrangeBall(scolor, carro, pala, compas, usensor, bar);	  
	  Behavior navegar 		= new DriveForward(carro);    
      Behavior esquivar 	= new HitWall(usensor,carro,compas, bar);
      Behavior aggregation  = new Agregation(carro, usensor);
      Behavior dispersion    = new Dispersion(carro, usensor);
      
      Arbitrator arby;
      
      //if(select == 0){ 
    	 // Behavior []  bArray1 = {navegar,aggregation, dispersion,esquivar,tirarAzul/*,tirarNaranja*/};
    	 // arby = new Arbitrator(bArray1);
    	  //.println("Todos los comportamientos");
    	 // color.calibrar();
      //}else if (select == 1){
    	//  Behavior [] bArray2 = {navegar,aggregation, dispersion,/*tirarAzul,tirarNaranja,*/esquivar};
    	 // arby = new Arbitrator(bArray2);
    	  //.println("Sin los colores");
      //}else{
    	 Behavior [] bArray3 = {navegar,aggregation, dispersion,tirarAzul,tirarNaranja,esquivar};
    	  arby = new Arbitrator(bArray3);
    	  scolor.calibrar();
    	  //scolor.calibrar();
     // }
      //Arbitrator arby = new Arbitrator(bArray);
      Delay.msDelay(5000);
      LCD.clear();
      bar.activar();
      //COMIENZA A ARBITRAR LOS COMPORTAMIENTOS
      arby.start();
     
      
      /*carro.resetTacho();
      carro.Forward();
      usensor.setMode(UltrasonicSensor.MODE_PING);
      usensor.ping(); 
 	  Delay.msDelay(200);
      int dist = usensor.getDistance();
      while(true){
    	 usensor.ping(); 
    	 Delay.msDelay(200);
    	 //usensor.ping();
    	 //Delay.msDelay(75)
    	 dist = usensor.getDistance();
    	 int tacho = carro.getTacho();
    	 //carro.resetTacho();
    	 .println(Integer.toString(dist)+":"+Integer.toString(tacho));
      }*/
   }
}

